from visual import *

import string
import math
import socket
import struct

#ser = serial.Serial(port='COM5', baudrate=115200, timeout=1)

dot = box(length=0.8, height=0.2, width=0.6, color=color.yellow)
xarrow = arrow(pos=(0,0,0), axis=(1,0,0), color=color.red)
yarrow = arrow(pos=(0,0,0), axis=(0,1,0), color=color.green)
zarrow = arrow(pos=(0,0,0), axis=(0,0,1), color=color.blue)
scene.forward = (1,-0.8,0)
scene.up=(0,0,1)
scene.lights = []
lamp=distant_light(direction=(-0.22,-0.44, 0.88), color=color.gray(0.6)) 
lamp=distant_light(direction=(0,1, 0), color=color.gray(0.8)) 
lamp=distant_light(direction=(-0.88, 0.22, 0.44), color=color.gray(0.3))


f = open("debug.txt", 'w')

UDP_IP="127.0.0.1"
UDP_PORT=9091

print "startin"

sock = socket.socket( socket.AF_INET, # Internet
                          socket.SOCK_DGRAM ) # UDP
sock.bind( (UDP_IP,UDP_PORT) )

while 1:
      #line = ser.readline()
      #words = string.split(line)
    data, addr = sock.recvfrom( 200 )
    
    words = string.split(data)

    w11 = float(words[0])
    w21 = float(words[1])
    w31 = float(words[2])
    w12 = float(words[3])
    w22 = float(words[4])
    w32 = float(words[5])
    w13 = float(words[6])
    w23 = float(words[7])
    w33 = float(words[8])

    
##    axis=(w33, w23, w13)
##    up=(w31, w21, w11)

    axis=(w11, w21, w31)
    up=(w13, w23, w33)

    dot.axis=axis
    dot.up=up
    xarrow.axis=axis
    xarrow.up=up
    yarrow.axis=axis
    yarrow.up=up
    yarrow.rotate(angle=radians(90), axis=up)

    zarrow.axis=axis
    zarrow.up=up
    zarrow.rotate(angle=radians(90), axis=up)
    zarrow.rotate(angle=radians(90), axis=axis)
    
   #f.write(line)
##    i = 0;
##    axis=(i, i+1, i+2)
##
##    up=(i+1, i+2, i+3)
##
##    i=i+1
##    xarrow.axis=axis
##    xarrow.up=up
##                
##    yarrow.axis=axis
##    yarrow.up=up
##    yarrow.rotate(angle=radians(90), axis=up)
##
##    zarrow.axis=axis
##    zarrow.up=up
##    zarrow.rotate(angle=radians(90), axis=up)
##    zarrow.rotate(angle=radians(90), axis=axis)
            
    #line = ser.readline()
    
   # f.write(line)
    
##    words = string.split(line)
##    if len(words) > 0:
##        if words[0] == "R" and len(words) == 10:
##            rm00 = int(words[1])/32767.*2.
##            rm01 = int(words[2])/32767.*2.
##            rm02 = int(words[3])/32767.*2.
##            rm10 = int(words[4])/32767.*2.
##            rm11 = int(words[5])/32767.*2.
##            rm12 = int(words[6])/32767.*2.
##            rm20 = int(words[7])/32767.*2.
##            rm21 = int(words[8])/32767.*2.
##            rm22 = int(words[9])/32767.*2.
##
##            axis=(rm00, rm10, rm20)
##            up=(rm02, rm12, rm22)
##
##            dot.axis=axis
##            dot.up=up
##                
##            xarrow.axis=axis
##            xarrow.up=up
##                
##            yarrow.axis=axis
##            yarrow.up=up
##            yarrow.rotate(angle=radians(90), axis=up)
##
##            zarrow.axis=axis
##            zarrow.up=up
##            zarrow.rotate(angle=radians(90), axis=up)
##            zarrow.rotate(angle=radians(90), axis=axis)

            #rm20_2 = rm20*rm20;
            #if rm20_2 > 1.:
            #    rm20_2 = 1.

            #s="axis:" + repr(axis) + '\n'
            #f.write(s)
            #s="up:" + repr(up) + '\n'
            #f.write(s)
            #s = repr( (rm00, rm01, rm02) ) + '\n' + repr( (rm10, rm11, rm12) ) + '\n' + repr( (rm20, rm21, rm22) ) + '\n\n'
            #f.write(s)

            #yaw = math.atan2(rm21, rm22)   # rotate about x0
            #pitch = math.atan2(-rm20, math.sqrt(1.-rm20_2)) # rotate about y0
            #roll = math.atan2(rm10, rm00) # rotate about z0

ser.close

